#include <stdio.h>
#include "pid.h"
#include "main.h"
#include "driver_tool.h"
#include "math.h"

typedef struct {
    float Kp;            // 比例系数
    float Ki;            // 积分系数
    float Kd;            // 微分系数
    float target_temp;   // 目标温度
    float integral;      // 积分累积值
    float prev_error;    // 上一次误差
    float max_output;    // PWM最大值（1000）
    uint8_t enable;
} PIDController;

PIDController pid;


/* PWM输出范围限制函数 */
float clamp(float value, float min, float max)
{
    if(value > max) return max;
    if(value < min) return min;
    return value;
}

/* PID初始化 */
void pid_init(float Kp, float Ki, float Kd, float target)
{
    pid.Kp = Kp;
    pid.Ki = Ki;
    pid.Kd = Kd;
    pid.target_temp = target;
    pid.integral = 0.0f;
    pid.prev_error = 0.0f;
    pid.max_output = 1000.0f; // PWM范围0-1000
}

/* PID计算函数（每秒调用一次）*/
float pid_compute(PIDController *pid, float target_temp, float current_temp)
{
    // 计算误差
    float error = target_temp - current_temp;

    // 比例项
    float proportional = pid->Kp * error;

    // 积分项（带防饱和）
    pid->integral += error;
    pid->integral = clamp(pid->integral, -pid->max_output, pid->max_output);
    float integral = pid->Ki * pid->integral;

    // 微分项（基于时间差1秒）
    float derivative = pid->Kd * (error - pid->prev_error);

    // 计算总输出
    float output = proportional + integral + derivative;

    // 输出限幅
    output = clamp(output, 0, pid->max_output);

    // 保存误差
    pid->prev_error = error;

    return output;
}

/* 模拟温度传感器读取 */
float read_temperature_sensor() {
    // 实际应用需替换为真实传感器驱动（如DS18B20）
    static float simulated_temp = 25.0f;
    simulated_temp += 0.5f; // 模拟温度变化
    return simulated_temp;
}


float pid_ctrl(float target_temp, float current_temp)
{
    float pwm = 0;

    // 初始化PID参数（需根据系统调试）
    pwm = pid_compute(&pid, target_temp, current_temp);
    // 调试输出
    printf("PID:%.1f,%.1f,%.1f\r\n", target_temp, current_temp, pwm);

    return pwm;
}


uint16_t user_pid_conctrl(float tar_data, float now_data)
{
	float out_data = 0;
    if (pid.enable == 1) {
        out_data = pid_ctrl(tar_data, now_data);
        uint16_t pwm = (uint16_t)fabs(out_data);
        // GUA_LOGI("pid tar[%.1f] now[%.1f] pwm[%d]", tar_data, now_data, pwm);
        return pwm;
    } else {
        return 0;
    }

}



void user_enable_pid(uint8_t flag, float target)
{
    if (flag) {
        GUA_LOGI("pid enable! target:[%.1f]", target);
        pid.enable = 1;
        pid_init(150.0f, 2.0f, 30.0f, target);
    } else {
        pid_init(0, 0, 0, target);
    }
}

